// Standard includes
#include <termios.h>
#include <fcntl.h>
#include <iostream>
using std::cout;
using std::cerr;
using std::endl;

// Local includes
#include "ieee.h"

#ifndef _IMU_H_
#define _IMU_H_

// Constants
#define G 9.80665

// Typedefs
typedef int ComPortHandle;

// Commands specified in communication protocol
const unsigned int RAW_ACCELEROMETER_AND_ANGULAR_RATE 					= 0xC1;
const unsigned int ACCELERATION_AND_ANGULAR_RATE 					= 0xC2;
const unsigned int DELTA_ANGLE_AND_DELTA_VELOCITY 					= 0xC3;
const unsigned int SET_CONTINUOUS_MODE 							= 0xC4;
const unsigned int ORIENTATION_MATRIX 							= 0xC5;
const unsigned int ORIENTATION_UPGRADE_MATRIX 						= 0xC6;
const unsigned int MAGNETOMETER_VECTOR 							= 0xC7;
const unsigned int ACCELERATION_AND_ANGULAR_RATE_AND_ORIENTATION_MATRIX 		= 0xC8;
const unsigned int WRITE_ACCEL_BIAS_CORRECTION 						= 0xC9;
const unsigned int WRITE_GYRO_BIAS_CORRECTION 						= 0xCA;
const unsigned int ACCELERATION_AND_ANGULAR_RATE_AND_MAGNETOMETER 			= 0xCB;
const unsigned int ACCELERATION_AND_ANGULAR_RATE_AND_MAGNETOMETER_AND_ORIENTATION 	= 0xCC;
const unsigned int CAPTURE_GYRO_BIAS 							= 0xCD;
const unsigned int EULER_ANGLES								= 0xCE;
const unsigned int EURLER_ANGLES_AND_ANGULAR_RATES					= 0xCF;
const unsigned int TRANSFER_QUANTITY_TO_NON_VOLATILE_MEMORY				= 0xD0;
const unsigned int TEMPERATURES								= 0xD1;
const unsigned int GYRO_STABILIZED_ACCELERATION_AND_ANGULAR_RATE_AND_MAGNETOMETER	= 0xD2;
const unsigned int DELTA_ANGLE_AND_DELTA_VELOCITY_AND_MAGNETOMETER_VECTORS		= 0xD3;
const unsigned int MODE									= 0xD4;
const unsigned int MODE_PRESET								= 0xD5;
const unsigned int CONTINUOUS_PRESET							= 0xD6;
const unsigned int TIMER								= 0xD7;
const unsigned int COMMUNICATIONS_SETTINGS						= 0xD9;
const unsigned int STATIONARY_TEST							= 0xDA;
const unsigned int SAMPLING_SETTINGS							= 0xDB;
const unsigned int REALIGN_UP_AND_NORTH							= 0xDD;
const unsigned int QUATERNION								= 0xDF;
const unsigned int WRITE_WORD_TO_EEPROM							= 0xE4;
const unsigned int READ_WORD_FROM_EEPROM						= 0xE5;
const unsigned int READ_FIRMWARE_VERSION_NUMBER						= 0xE9;
const unsigned int READ_DEVICE_ID_STRING						= 0xEA;
const unsigned int STOP_CONTINUOS_MODE							= 0xFA;
const unsigned int FIRMWARE_UPDATE							= 0xFD;
const unsigned int DEVICE_RESET								= 0xFE;

// IMUData
struct IMUData {
	double roll;		// degrees
	double pitch;		// degrees
	double yaw;		// degrees
	double dxVeloc;		// m/s
	double dyVeloc;		// m/s
	double dzVeloc;		// m/s
};

// IMU
class IMU {	
	public:
		// openComPort
		// Opens a com port with the correct settings for communicating with a MicroStrain 3DM-GX3-25 sensor
		bool openComPort(const char *comPortPath) {
			comPort = open(comPortPath, O_RDWR | O_NOCTTY);

			if (comPort== -1) { //Opening of port failed
				cerr << "Unable to open port: " << comPortPath << endl;
				return false;
			}

			// Get the current options for the port...
			struct termios options;
			tcgetattr(comPort, &options);

			// Set the baud rate to 115200
			int baudRate = B115200;
			cfsetospeed(&options, baudRate);
			cfsetispeed(&options, baudRate);

			// Set the number of data bits.
			options.c_cflag &= ~CSIZE;  // Mask the character size bits
			options.c_cflag |= CS8;

			// Set the number of stop bits to 1
			options.c_cflag &= ~CSTOPB;

			// Set parity to None
			options.c_cflag &=~PARENB;

			// Set for non-canonical (raw processing, no echo, etc.)
			options.c_iflag = IGNPAR; // ignore parity check close_port(int)
			options.c_oflag = 0; // raw output
			options.c_lflag = 0; // raw input

			// Time-Outs -- won't work with NDELAY option in the call to open
			options.c_cc[VMIN]  = 0;   // block reading until RX x characers. If x = 0, it is non-blocking.
			options.c_cc[VTIME] = 100;   // Inter-Character Timer -- i.e. timeout= x*.1 s

			// Set local mode and enable the receiver
			options.c_cflag |= (CLOCAL | CREAD);

			// Purge serial port buffers
			purge();

			// Set the new options for the port...
			int status=tcsetattr(comPort, TCSANOW, &options);

			if (status != 0) { //For error message
				cerr << "Configuring comport failed" << endl;
				return status;
			}

			// Purge serial port buffers
			purge();

			return true;
		}

		// purge
		// Clears the com port's read and write buffers
		bool purge() {
			if (tcflush(comPort,TCIOFLUSH) == -1) {
				cerr << "Flush failed" << endl;
				return false;
			}

			return true;
		}

		// closeComPort
		// Closes a port that was previously opened with openComPort
		void closeComPort() {
			close(comPort);
		}

		// readComPort
		// read the specivied number of bytes from the com port
		int readComPort(Byte* bytes, int bytesToRead) {
			int bytesRead = read(comPort, bytes, bytesToRead);
			return bytesRead;   
		}


		// writeComPort
		// send bytes to the com port
		int writeComPort(unsigned char* bytesToWrite, int size) {
			return write(comPort, bytesToWrite, size);
		}

		// isOpen
		// determine if the com port for the IMU has been opened
		bool isOpen() {
			if(comPort > 0) {
				return true;
			} else {
				return false;
			}
		}

		// readData
		// send commands to IMU to read the appropriate data
		void readData(IMUData &data) {
			Byte response[4096] = { 0 };

			// Get roll, pitch, and yaw
			unsigned char command = (char)EULER_ANGLES;
			writeComPort(&command, 1); purge();
			readComPort(&response[0], 4096); purge();

			data.roll = convertToDecimal(response, 1, 4) * 180 / PI;
			data.pitch = convertToDecimal(response, 5, 4) * 180 / PI;
			data.yaw = convertToDecimal(response, 9, 4) * 180 / PI;

			// Get delta velocity
			command = (char)DELTA_ANGLE_AND_DELTA_VELOCITY;
			writeComPort(&command, 1); purge();
			readComPort(&response[0], 4096); purge();
			
			data.dxVeloc = convertToDecimal(response, 13, 4);
			data.dyVeloc = convertToDecimal(response, 17, 4);
			data.dzVeloc = convertToDecimal(response, 21, 4);
		}

	private:
		ComPortHandle comPort;
};

#endif
